/*****************************************************************************
 * $LastChangedDate: 2013-07-14 09:20:40 -0400 (Sun, 14 Jul 2013) $
 * @file
 * @author  Jim E. Brooks  http://www.palomino3d.org
 * @brief   Matrix functions (OSG).
 * @remarks
 * Matrix functions are template functions because a few functions
 * in the OSG API operate on Matrixd (not Matrixf) so flexibility is needed.
 *//*
 * LEGAL:   COPYRIGHT (C) 2007 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE VERSION 2 (GPL 2).
 *****************************************************************************/

#ifndef MATH_MATRIX_FUNCS_HH
#define MATH_MATRIX_FUNCS_HH 1

#include "base/stream.hh"
#include "math/funcs_vector.hh"
#include "math/funcs_osg.hh"

namespace math {

/*****************************************************************************
 * Reset a matrix to identity (no rotation, no translation).
 *****************************************************************************/
template<typename MATRIX>
void
MatrixIdentity( MATRIX& m /*IN/OUT*/ )
{
    m[Xx] = m[Xy] = m[Xz] = m[Xw] = \
    m[Yx] = m[Yy] = m[Yz] = m[Yw] = \
    m[Zx] = m[Zy] = m[Zz] = m[Zw] = \
    m[Ox] = m[Oy] = m[Oz]         = 0.0f;
    m[Xx] = m[Yy] = m[Zz] = m[Ow] = 1.0f;  // diagonal=1
}

/*****************************************************************************
 * Translate a Matrix by a vector offset.
 *****************************************************************************/
template<typename MATRIX>
void
MatrixTranslate( MATRIX& m /*IN/OUT*/, const Vector3& offset )
{
    MATRIX translation;
    translation.makeTranslate( offset );
    m = translation * m;
}

/*****************************************************************************
 * Translate a matrix along an axis in fixed space.
 * For translating the eye/viewpoint.
 *****************************************************************************/
template<typename MATRIX,typename FPM>
void
MatrixTranslateFixed( MATRIX& m /*IN/OUT*/, const uint axis, const FPM inc )
{
ASSERT_AXIS3( axis );
    MATRIX translation;
    translation.makeTranslate( Axis2Vec3(axis,1) * inc );
    m = m * translation;
}

/*****************************************************************************
 * Translate a matrix along its own axis.
 * For translating a Dyna.
 *****************************************************************************/
template<typename MATRIX,typename FPM>
void
MatrixTranslateLocal( MATRIX& m /*IN/OUT*/, const uint axis, const FPM inc )
{
ASSERT_AXIS3( axis );
    MATRIX translation;
    translation.makeTranslate( Axis2Vec3(axis,1) * inc );
    m = translation * m;
}

template<typename MATRIX,typename VECTOR3>
void
MatrixTranslateLocal( MATRIX& m /*IN/OUT*/, const VECTOR3& v )
{
    MATRIX translation;
    translation.makeTranslate( v );
    m = translation * m;
}

/*****************************************************************************
 * Rotate a matrix around a fixed axis.
 * This function is suited to rotating the viewpoint/eye.
 * The word "fixed" should be clear by looking at the math.
 * Eg, the value of the X coord won't be changed by a rotation around the X axis.
 *****************************************************************************/
template<typename MATRIX>
void
MatrixRotateFixed( MATRIX& m /*IN/OUT*/, const uint axis, const Radian rad )
{
ASSERT_AXIS3( axis );
    MATRIX rotation;
    rotation.makeRotate( rad, Axis2Vec3(axis) );
    m = m * rotation;  // rotate around fixed axis
}

/*****************************************************************************
 * Rotate matrix around local axis.
 * Rotate a local coordinate system around its own axis.
 * This function is suited to rotating an independent object.
 * The rotation is relative to local coodinate system (not the fixed/eye system).
 * The trick is to load an identity-mapped matrix and rotate it, then transform
 * it thru the given matrix (which defines the local coordinate system)
 * as though it was the coords (1.0, 1.0, 1.0).  These coords of course define
 * the X,Y,Z axises.  The transformation is effectively a local rotation.
 *****************************************************************************/
template<typename MATRIX>
void
MatrixRotateLocal( MATRIX& m /*IN/OUT*/, const uint axis, const Radian rad )
{
ASSERT_AXIS3( axis );
    MATRIX rotation;
    rotation.makeRotate( rad, Axis2Vec3(axis) );
    m = rotation * m;  // rotate around local axis
}

/*****************************************************************************
 * -- Alternative is MatrixTranspose() --
 * Reverse the mapping between two coordinate systems.
 * Math notes:
 * osg::Matrix::inverse() is more complex than a purely mathematical function.
 *****************************************************************************/
template<typename MATRIX>
MATRIX
MatrixInvert( const MATRIX& src )
{
    return MATRIX::inverse( src );  // osg::Matrix::inverse()
}

/*****************************************************************************
 * -- Alternative is MatrixInvert() --
 * Reverse the mapping between two coordinate systems by transposing matrix rows/columns.
 * dest and src must be different objects.
 * Math notes: This produces different results than MatrixInvert().
 *****************************************************************************/
template<typename MATRIX>
void
MatrixTranspose( MATRIX& dest/*OUT*/, const MATRIX& src )
{
ASSERT( &dest != &src );

    // Transpose rotation sub-matrix.
    // X = X'
    dest[Xx] = src[Xx];
    dest[Xy] = src[Yx];
    dest[Xz] = src[Zx];
    // Y = Y'
    dest[Yx] = src[Xy];
    dest[Yy] = src[Yy];
    dest[Yz] = src[Zy];
    // Z = Z'
    dest[Zx] = src[Xz];
    dest[Zy] = src[Yz];
    dest[Zz] = src[Zz];

    // Negate origin sub-matrix.
    dest[Ox] = -src[Ox];
    dest[Oy] = -src[Oy];
    dest[Oz] = -src[Oz];
}

/*****************************************************************************
 * Distance^2 between two matrixs.
 * Don't pass different kinds of matrixs (eg Eye origin is negative but Dyna is positive).
 *****************************************************************************/
template<typename MATRIX1, typename MATRIX2>
fpx
MatrixDistanceSquared( const MATRIX1& m1, const MATRIX2& m2 )
{
    Vector3 v1( m1[Ox], m1[Oy], m1[Oz] );
    Vector3 v2( m2[Ox], m2[Oy], m2[Oz] );
    return DistanceSquared( v1, v2 );
}

void
MatrixScale( osg::MatrixTransform& transform /*IN/OUT*/, const Vector3& scale );

void
MatrixTranslate( osg::MatrixTransform& transform /*IN/OUT*/, const Vector3& offset );

void
MatrixRotateLocal( osg::MatrixTransform& transform /*IN/OUT*/, const uint axis, const Radian rad );

void
MatrixRotateFixed( osg::MatrixTransform& transform /*IN/OUT*/, const uint axis, const Radian rad );

std::ostream& operator<<( std::ostream& strm, const Matrix& m );

} // namespace math

#endif // MATH_MATRIX_FUNCS_HH
